#include "data_holder.h"
#include "board.h"
#include "util.h"
#include <stdio.h>

// Data_holder::Data_holder()
// {
//     sprintf(firmware_info.version, "%d.%d.%d", FIRMWARE_VERSION_MAJOR, FIRMWARE_VERSION_MINOR, FIRMWARE_VERSION_REVISION);
//     sprintf(firmware_info.date, "%s", BUILD_DATE);
//     sprintf(firmware_info.time, "%s", BUILD_TIME);
//     sprintf(robot_info.model, "%s", MODEL_NAME);
//     memset(&control_cmd, 0, sizeof(struct Robot_control_cmd));
//     memset(&pid_data, 0, sizeof(struct Robot_pid_data));
//     memset(&imu_data, 0, sizeof(struct Robot_imu_data));
//     memset(&quaternion_data, 0, sizeof(struct Robot_quaternion_data));
//     memset(&depth_data, 0, sizeof(struct Robot_depth_data));
//     memset(&dynamic_parameter, 0, sizeof(struct Robot_dynamic_parameter));
//     // memset(&data_records, 0, sizeof(struct Robot_data_records));
//     memset(&manual_control, 0, sizeof(struct Robot_manual_control));
//     memset(&power_data, 0, sizeof(struct Robot_power_data));
//     memset(&bluetooth_parameter, 0, sizeof(struct Robot_bluetooth_parameter));
//     memset(&action_parameter, 0, sizeof(struct Robot_action_parameter));
//     // memset(&robot_communication_fault_code, 0, sizeof(struct Robot_communication_fault_code));
//     memset(&state_parameter, 0, sizeof(struct Robot_state_parameter));
//     memset(&heartbeat_state, 0, sizeof(struct Device_state));
//     memset(robot_fault_alarm, 0, FAULT_RECORDS_SIZE * sizeof(struct Robot_fault_alarm));
//     memset(&serial_number_data, 0, sizeof(struct Serial_number_data));
//     memset(&serial_number_result, 0, sizeof(struct Serial_number_result));

//     robot_info.chip = MCU_MODEL;
//     robot_info.board_model = BOARD_MODEL;

//     heartbeat_state.owned_devices.main_control_device = 1;
//     heartbeat_state.online_devices.main_control_device = 1;
//     heartbeat_state.reboot_devices.main_control_device = 1;
// #if DEPTH_SENSOR_ENABLE
//     // heartbeat_state.owned_devices.depth_device = 1;
// #endif
// #if LED_TYPE == LED_TYPE_MODULE
//     heartbeat_state.owned_devices.led_device = 1;
// #endif

//     product_info.board_type = BOARD_TYPE;
//     product_info.product_model = ROBOT_MODEL;
//     product_info.mcu_model = MCU_MODEL;
//     product_info.hw_version = HARDWARE_VERSION;
//     product_info.fw_version[0] = FIRMWARE_VERSION_MAJOR;
//     product_info.fw_version[1] = FIRMWARE_VERSION_MINOR;
//     product_info.fw_version[2] = FIRMWARE_VERSION_REVISION;

//     debug_pose_parameter = &(basic_parameter.pose_parameter);
//     debug_alarm_parameter = &(basic_parameter.alarm_parameter);
//     debug_event_configuration = &(basic_parameter.event_configuration);
//     debug_motor_configuration = &(basic_parameter.motor_configuration);
//     debug_function_configuration = &(basic_parameter.function_configuration);
//     debug_filter_parameter = &(basic_parameter.filter_parameter);
//     debug_floor_PID_parameter = &(basic_parameter.floor_PID_parameter);
//     debug_wall_PID_parameter = &(basic_parameter.wall_PID_parameter);
//     debug_mode = &(state_parameter.robot_mode);
//     pose_data = &(state_parameter.pose_data);

//     robot_calibration_parameter.ax_offset = 0;
//     robot_calibration_parameter.ay_offset = 0;
//     robot_calibration_parameter.az_offset = 0;
//     robot_calibration_parameter.gx_offset = 0;
//     robot_calibration_parameter.gy_offset = 0;
//     robot_calibration_parameter.gz_offset = 0;
//     robot_calibration_parameter.k_power_voltage = 0.0f;
//     robot_calibration_parameter.b_power_voltage = 0;
//     robot_calibration_parameter.k_pump_current = 3.22f;
//     robot_calibration_parameter.b_pump_current = 0;
//     robot_calibration_parameter.k_motor1_current = 2.00f;
//     robot_calibration_parameter.b_motor1_current = 0;
//     robot_calibration_parameter.k_motor2_current = 2.00f;
//     robot_calibration_parameter.b_motor2_current = 0;
//     robot_calibration_parameter.imu_calibrated_flag = 0;
//     robot_calibration_parameter.power_voltage_calibrated_flag = 0;
//     robot_calibration_parameter.pump_current_calibrated_flag = 0;
//     robot_calibration_parameter.motor1_current_calibrated_flag = 0;
//     robot_calibration_parameter.motor2_current_calibrated_flag = 0;
//     robot_calibration_parameter.calibration_finished_flag = 0;
//     robot_calibration_parameter.depth_reference_calibrated_flag = 0;
//     robot_calibration_parameter.depth_reference_value = 10300;


//     basic_parameter.pose_parameter.max_angle_for_on_floor_state = 35;
//     basic_parameter.pose_parameter.max_angle_for_tilt_state = 70;
//     basic_parameter.pose_parameter.max_angle_for_on_wall_state = 125;
//     basic_parameter.pose_parameter.target_angle_for_washing_on_wall = 15;                       // 池壁清洗倾斜角度 °

//     basic_parameter.alarm_parameter.walk_motor_current_for_short_circuit = 6000;                //  行走短路 电流阈值 默认值 >6000mA
//     basic_parameter.alarm_parameter.pump_motor_current_for_short_circuit = 8000;                //  水泵短路 电流阈值 默认值 >8000mA
//     basic_parameter.alarm_parameter.pump_motor_current_for_overload = 6000;                     //  水泵过载 电流阈值 默认值 >6000mA
//     basic_parameter.alarm_parameter.pump_motor_current_for_out_water = 1700;                    //  水泵出水 电流阈值 默认值 <3500mA
//     basic_parameter.alarm_parameter.walk_motor_differential_current_for_touching_wall = 225;    //  行走触壁监测 电流差值阈值 默认值
//     basic_parameter.alarm_parameter.walk_motor_accumulated_current_for_touching_wall = 1500;    //  行走触壁监测 电流积分阈值 默认值
//     basic_parameter.alarm_parameter.walk_motor_differential_current_for_overload = 500;         //  行走过载 电流差值阈值 默认值
//     basic_parameter.alarm_parameter.walk_motor_accumulated_current_for_overload = 25000;        //  行走过载 电流积分阈值
//     basic_parameter.alarm_parameter.pump_motor_differential_current_for_suck_air = -150;        //  水泵进气 电流差值阈值
//     basic_parameter.alarm_parameter.pump_motor_accumulated_current_for_suck_air = -600;         //  水泵进气 电流积分阈值

//     basic_parameter.event_configuration.differential_pwm_for_reaching_water_line = 1000;            //  爬壁监测 PWM阈值
//     basic_parameter.event_configuration.differential_pwm_for_touching_wall = 150;                   //  触壁监测 PWM阈值
//     basic_parameter.event_configuration.timeout_for_detecting_touch_wall_on_floor = 5000;           //  触壁后，仍在池底无法爬墙超时时间 ms
//     basic_parameter.event_configuration.time_for_detecting_touch_wall_by_pwm = 1500;                //  PWM监测触壁时间阈值 ms
//     basic_parameter.event_configuration.delay_time_for_detecting_touch_wall = 8000;
//     basic_parameter.event_configuration.gz_variance_gain_for_touching_wall = 20;                       //  触壁检测 gyro 方差阈值 默认值 20
//     basic_parameter.event_configuration.depth_variance_for_stable = 70;
//     basic_parameter.event_configuration.infrared_distance_for_touching_wall = 150;
//     basic_parameter.event_configuration.infrared_distance_for_reaching_waterline = 60;

//     basic_parameter.function_configuration.accel_time = 2000;                                       //  行走平均PWM，加速时间ms

//     basic_parameter.function_configuration.max_washing_wall_time = 20000;                           //  池壁清洗最长时间 ms
//     basic_parameter.function_configuration.cmd_last_time = 2000;                                    //  命令持久时间ms 超过该时间会自动停止运动
    

//     basic_parameter.function_configuration.imu_sensor_enable = true;

// #if DEPTH_SENSOR_ENABLE
//     basic_parameter.function_configuration.depth_sensor_enable = true;
// #else
//     basic_parameter.function_configuration.depth_sensor_enable = false;
// #endif

// // #if INFRARED_SENSOR_ENABLE
// //     basic_parameter.function_configuration.infrared_sensor_enable = true;
// // #else
//     basic_parameter.function_configuration.infrared_sensor_enable = false;
// // #endif

// #if SUCK_AIR_ACTION_ENABLE
//     basic_parameter.function_configuration.suck_air_action_enable = true;
// #else
//     basic_parameter.function_configuration.suck_air_action_enable = false;
// #endif

// #if PUMP_SPOUT_ACTION_ENABLE
//     basic_parameter.function_configuration.pump_spout_action_enable = true;
// #else
//     basic_parameter.function_configuration.pump_spout_action_enable = false;
// #endif

// #if PUMP_OVERLOAD_ACTION_ENABLE
//     basic_parameter.function_configuration.pump_overload_action_enable = true;
// #else 
//     basic_parameter.function_configuration.pump_overload_action_enable = false;
// #endif

// #if WALKING_OVERLOAD_ACTION_ENABLE
//     basic_parameter.function_configuration.walking_overload_action_enable = true;
// #else
//     basic_parameter.function_configuration.walking_overload_action_enable = false;
// #endif
//     basic_parameter.function_configuration.stuck_on_floor_action_enable = true;
//     basic_parameter.function_configuration.down_wall_stuck_action_enable = true;
//     basic_parameter.function_configuration.tilt_stuck_action_enable = true;
    
// #if BUMP_TOUCH_WALL_DETECTION_ENABLE == 1
//     basic_parameter.function_configuration.variance_touch_wall_detection_function_enable = true;
// #else
//     basic_parameter.function_configuration.variance_touch_wall_detection_function_enable = false;
// #endif

// #if POWER_BALANCE_ENABLE == 1
//     basic_parameter.function_configuration.power_balance_function_enable = true;
// #else
//     basic_parameter.function_configuration.power_balance_function_enable = false;
// #endif
//     basic_parameter.filter_parameter.filter_coefficient_adc = 200;                              //  系数 0.2 ，对应5个数据的平均
//     basic_parameter.filter_parameter.filter_coefficient_current_baseline = 1;                   //  系数0.001 ，对应1000个数据的平均

//     basic_parameter.floor_PID_parameter.do_pid_interval = 20; //pid间隔 (ms)
//     basic_parameter.floor_PID_parameter.T= 20;
//     basic_parameter.floor_PID_parameter.k_p = 1000;
//     basic_parameter.floor_PID_parameter.T_i = 2000;
//     basic_parameter.floor_PID_parameter.T_d = 1;
//     basic_parameter.floor_PID_parameter.k_o = 1; //pid参数比例

//     basic_parameter.wall_PID_parameter.do_pid_interval = 20; //pid间隔 (ms)
//     basic_parameter.wall_PID_parameter.T = 20;
//     basic_parameter.wall_PID_parameter.k_p = 3000;
//     basic_parameter.wall_PID_parameter.T_i = 500;
//     basic_parameter.wall_PID_parameter.T_d = 1;
//     basic_parameter.wall_PID_parameter.k_o = 1;

//     basic_parameter.motor_configuration.max_pwm = 1000;                                             //  电机最大PMW值
//     basic_parameter.motor_configuration.min_pwm = 150;                                              //  电机运动的最小PWM值
//     basic_parameter.motor_configuration.max_differential_pwm_on_floor = 300;                        //  池底直线运动状态，左右行走电机最大差速PWM值
//     basic_parameter.motor_configuration.max_differential_pwm_on_wall = 200;                         //  池壁状态，左右行走电机最大差速PWM值
//     basic_parameter.motor_configuration.max_pwm_change_time = 1000;                                 //  PWM 增加到最大PMW的变化时间ms
    
//     basic_parameter.motor_configuration.walk_motor_pwm_baseline_floor = WALKING_MOTOR_PWM_BASELINE_FLOOR;
//     basic_parameter.motor_configuration.walk_motor_pwm_baseline_up_wall = WALKING_MOTOR_PWM_BASELINE_UP_WALL;
//     basic_parameter.motor_configuration.walk_motor_pwm_baseline_down_wall = WALKING_MOTOR_PWM_BASELINE_DOWN_WALL;
//     basic_parameter.motor_configuration.walk_motor_pwm_baseline_waterline = WALKING_MOTOR_PWM_BASELINE_WATERLINE;

//     basic_parameter.motor_configuration.pump_motor_pwm_baseline_floor = PUMP_MOTOR_PWM_BASELINE_FLOOR;
//     basic_parameter.motor_configuration.pump_motor_pwm_baseline_up_wall = PUMP_MOTOR_PWM_BASELINE_UP_WALL;
//     basic_parameter.motor_configuration.pump_motor_pwm_baseline_down_wall = PUMP_MOTOR_PWM_BASELINE_DOWN_WALL;
//     basic_parameter.motor_configuration.pump_motor_pwm_baseline_waterline = PUMP_MOTOR_PWM_BASELINE_WATERLINE;

//     control_cmd.floor_route_mode = FLOOR_CROSS_WASHING_ROUTE;
//     control_cmd.wall_route_mode = NO_WALL_ROUTE;
//     control_cmd.operation_mode = STANDBY;
//     control_cmd.action = MOVE_STRAIGHT_ON_FLOOR;

// #if POWER_SUPPLY_METHOD == BATTERY_POWER_SUPPLY
//     state_parameter.robot_mode.robot_operation_mode = NO_MODE;
// #elif POWER_SUPPLY_METHOD == DC_POWER_SUPPLY || POWER_SUPPLY_METHOD == FLOATING_BATTERY_POWER_SUPPLY
//     state_parameter.robot_mode.robot_operation_mode = SELF_CHECK;
// // #elif POWER_SUPPLY_METHOD == DC_POWER_AND_BATTERY_BOX_POWER_SUPPLY
// //     state_parameter.robot_mode.robot_operation_mode = STANDBY;
// #endif
//     /**
//      *   清洗模式                       池底路径                                            池壁路径
//      *   FLOOR_MODE（3172系列）         INITIAL_CROSS_ROUTE -> FLOOR_STAR_ROUTE            NO_WALL_ROUTE
//      *   FLOOR_MODE（3092系列）         FLOOR_CROSS_WASHING_ROUTE                          NO_WALL_ROUTE
//      *   WALL_MODE （3172系列）         INITIAL_CROSS_ROUTE -> FLOOR_SQUARE_ROUTE          NO_WALL_ROUTE -> WALL_NORMAL_ROUTE
//      *   WALL_MODE （3092系列）         NO_FLOOR_ROUTE                                     WALL_H_ROUTE
//      *   ALL_POOL_MODE                 FLOOR_CROSS_WASHING_ROUTE                          WALL_NORMAL_ROUTE
//      * 
//      *   TWO_WALL_TWO_FLOOR_MODE       INITIAL_CROSS_ROUTE -> WALL_MODE （3172系列） -> FLOOR_MODE（3172系列）-> CENTER_CROSS_ROUTE-> WALL_MODE （3172系列） -> FLOOR_MODE（3172系列）
//      */

//     state_parameter.robot_mode.cleaning_mode = DEFAULT_CLEANING_MODE;
//     state_parameter.robot_mode.pool_shape_adaptive_route_enable = false;
//     state_parameter.robot_mode.floor_route_mode = FLOOR_CROSS_WASHING_ROUTE;
//     state_parameter.robot_mode.wall_route_mode = WALL_NORMAL_ROUTE;

//     // state_parameter.robot_mode.cleaning_mode = WALL_H_ROUTE_MODE;
//     // state_parameter.robot_mode.floor_route_mode = NO_FLOOR_ROUTE;
//     // state_parameter.robot_mode.wall_route_mode = WALL_H_ROUTE;

//     state_parameter.robot_mode.engineering_mode = IDLE;

//     state_parameter.total_start_floor_count = 0;
//     state_parameter.mode_start_floor_index = 0;
//     state_parameter.total_start_wall_count = 0;
//     state_parameter.mode_start_wall_index = 0;
//     state_parameter.touch_wall_detection_enable = false;
//     state_parameter.pwm_touch_wall_detection_enable = false;
//     state_parameter.touch_wall_detected = false;
//     state_parameter.pwm_touch_wall_detected = false;
//     state_parameter.inclination_touch_wall_detected = false;
//     state_parameter.touch_wall_detection_delay_enable = false;
//     state_parameter.variance_touch_wall_detection_enable = false;
//     state_parameter.variance_touch_wall_detected = false;
//     state_parameter.touch_wall_count = 0;
//     state_parameter.down_wall_stuck = false;
//     state_parameter.up_wall_stuck = false;
//     state_parameter.stuck_on_floor = false;
//     state_parameter.stuck_on_floor_detected_by_touch_wall_detector = false;
//     state_parameter.stuck_on_floor_detected_by_turn_direction_timeout_detector = false;
//     state_parameter.unable_tilt_wash_on_wall = false;
//     state_parameter.reach_top = false;
//     state_parameter.reach_bottom = false;
//     state_parameter.reach_waterline = false;
//     state_parameter.climb_wall = false;
//     state_parameter.need_detect_corner = false;

//     for(int i = 0; i < 10; i++)
//     {
//         state_parameter.on_floor_period[i] = 0;
//     }
//     state_parameter.direction_reversed = false;
//     // state_parameter.yaw_variance = 0;
//     state_parameter.pool_center_period = 0;
//     state_parameter.depth_stable = false;


//     state_parameter.pose_data.yaw = 0;
//     state_parameter.pose_data.pitch = 0;
//     state_parameter.pose_data.roll = 0;
//     state_parameter.pose_data.theta = 0;
//     state_parameter.pose_data.beta = 0;
//     state_parameter.pose_data.pose_state = ON_FLOOR;
//     state_parameter.pose_data.last_pose_state = ON_FLOOR;
//     state_parameter.out_of_water_action = false;
//     state_parameter.pump_spout_action = false;


//     action_parameter.action = STOP_ALL;
//     action_parameter.movement_direction = FORWARD;

//     pool_parameter.pool_length = 500;
//     pool_parameter.pool_width = 300;
//     pool_parameter.pool_depth = 200;
//     pool_parameter.pool_shape = POOL_SHAPE_RECTANGLE;
//     pool_parameter.pool_floor_type = POOL_FLOOR_TYPE_SLOPING;
//     pool_parameter.pool_floor_max_angle = 30;

//     depth_data.depth = 0;
//     depth_data.min_depth = 20000;
//     depth_data.max_depth = 0;
//     depth_data.climb_speed = 0;

//     temperature_data.temperature = 0;
//     temperature_data.temperature_bias = 0;

//     imu_calibration_success = 0;
//     depth_calibration_success = 0;

//     power_simulation.simulation_enable = false;
//     power_simulation.power_voltage = 0;
//     power_simulation.left_motor_current = 0;
//     power_simulation.right_motor_current = 0;
//     power_simulation.pump_motor_current = 0;

// #if LAGACY_BLUETOOTH_ENABLE
//     bluetooth_data.cleaning_mode = FLOOR_CLEANING_MODE;
//     bluetooth_data.pool_shape = POOL_SHAPE_RECTANGLE;
//     bluetooth_data.pool_floor_type = POOL_FLOOR_TYPE_SLOPING;
//     bluetooth_data.pool_length = 0;
//     bluetooth_data.pool_width = 0;
// #endif

//     schedule_state.schedule_config.schedule_enable = false;
//     schedule_state.schedule_config.interval_hours = 24;
//     schedule_state.schedule_config.cycle_time_minutes = 60;
//     schedule_state.rtc_error = false;
//     schedule_state.rtc_power_down_detected = false;
// // #if DEBUG_ENABLE
// //     interface_dataframe_type[DEBUG_USART] = INTERFACE_DATAFRAME_TYPE_DEBUG;
// // #endif
// // #if IOT_ENABLE
// //     interface_dataframe_type[IOT_USART] = INTERFACE_DATAFRAME_TYPE_IOT;
// // #endif
//     floor_s_route_stage = FLOOR_S_ROUTE_MOVE_FORWARD;
//     next_floor_s_route_stage = FLOOR_S_ROUTE_TURN_RIGHT;

//     serial_number_data.device_type = BOARD_TYPE;
//     serial_number_result.device_type = BOARD_TYPE;
//     device_name_data.device_type = BOARD_TYPE;
//     strcpy(device_name_data.device_name, MODEL_NAME);

//     dynamic_parameter.motor_pwm_factor = 1;
//     dynamic_parameter.pump_pwm_factor = 1;
//     app_connected = false;
//     production_test_button_triggered = false;
//     production_test_result.state = ENGINEERING_ACTION_STATE_IDLE;

//     area_washing_state.area_washing_coverage = SMALL_COVERAGE_RANGE;
//     area_washing_state.area_washing_progress = AREA_WASHING_FINISH;
//     // 机器类型
//     function_info.robot_type = POWER_SUPPLY_METHOD;
//     // 手动模式
// #if POWER_SUPPLY_METHOD == DC_POWER_SUPPLY || POWER_SUPPLY_METHOD == FLOATING_BATTERY_POWER_SUPPLY
//     function_info.manual_control = 1;
//     function_info.auto_return = 1;
//     function_info.area_washing = 1;
// #endif
//     // 传感器信息
//     function_info.pose_sensor = 1;
// #if INFRARED_SENSOR_ENABLE
//     function_info.infrared_sensor = 1;
// #endif
// #if DEPTH_SENSOR_ENABLE
//     function_info.temperature_sensor = 1;
//     function_info.depth_sensor = 1;
// #endif
//     // 时间及预约模式
// #if POWER_SUPPLY_METHOD == DC_POWER_SUPPLY || POWER_SUPPLY_METHOD == FLOATING_BATTERY_POWER_SUPPLY
//     function_info.weekly_schedule = 1;
// #endif
// #if SUPPORT_SCHEDULE
//     function_info.interval_schedule_by_cycle_time = 1;
//     function_info.interval_schedule_by_cycle_count = 1;
// #endif
// #if ROBOT_MODEL == WY3051SP
//     function_info.smart_cleaning = 1;
// #endif
// #if POWER_SUPPLY_METHOD == DC_POWER_SUPPLY
//     function_info.delay_cleaning = 1;
//     function_info.cycle_time_config = 1;
// #endif
//     // 池底模式
//     function_info.floor_star_route_mode = FLOOR_STAR_ROUTE_MODE_ENABLE;
//     function_info.floor_cross_route_mode = FLOOR_CROSS_ROUTE_MODE_ENABLE;
// // #if ROBOT_MODEL == WY3051 || ROBOT_MODEL == WY3051SP || ROBOT_MODEL == WY3092 || ROBOT_MODEL == WY3151 || ROBOT_MODEL == WYBOT_S1
//     function_info.floor_S_route_mode = FLOOR_S_ROUTE_MODE_ENABLE;
//     // 池壁模式
//     function_info.wall_normal_route_mode = WALL_NORMAL_ROUTE_MODE_ENABLE;
//     function_info.wall_N_route_mode = WALL_N_ROUTE_MODE_ENABLE;
// // #if ROBOT_MODEL == WY3051 || ROBOT_MODEL == WY3051SP || ROBOT_MODEL == WY3092 || ROBOT_MODEL == WY3151
//     function_info.wall_H_route_mode = WALL_H_ROUTE_MODE_ENABLE;
// // #endif
//     // 组合模式
//     function_info.twice_wall_twice_floor_mode = TWICE_WALL_TWICE_FLOOR_ROUTE_MODE_ENABLE;
// // #if ROBOT_MODEL == WY3312PRO || ROBOT_MODEL == WY3312PRO_S || ROBOT_MODEL == WY3312MAX_S || ROBOT_MODEL == HJ3182P || ROBOT_MODEL == WYBOT_S1 || ROBOT_MODEL == WY3151
//     function_info.once_wall_once_floor_mode = ONCE_WALL_ONCE_FLOOR_ROUTE_MODE_ENABLE;
// // #endif
//     // 全池模式
//     function_info.full_pool_star_route_mode = ALL_POOL_STAR_ROUTE_MODE_ENABLE;
//     function_info.full_pool_cross_route_mode = ALL_POOL_CROSS_ROUTE_MODE_ENABLE;
//     // 水线模式
// // #if ROBOT_MODEL == WY3051 || ROBOT_MODEL == WY3051SP || ROBOT_MODEL == WY3092 || ROBOT_MODEL == WY3151 || ROBOT_MODEL == WYBOT_S1 || ROBOT_MODEL == HJ3052P
//     function_info.waterline_route_mode = WATERLINE_ROUTE_MODE_ENABLE;
// // #endif
//     // 强力池底模式
// // #if ROBOT_MODEL == WYBOT_S1
//     function_info.powerful_floor_star_route_mode = TURBO_FLOOR_STAR_ROUTE_MODE_ENABLE;
//     function_info.powerful_floor_S_route_mode = TURBO_FLOOR_S_ROUTE_MODE_ENABLE;
//     function_info.powerful_floor_cross_route_mode = TURBO_FLOOR_CROSS_ROUTE_MODE_ENABLE;
// // #endif
// }

// void Data_holder::load_parameter()
// {
// #if EEPROM_ENABLE
//     EEPROMBase *eeprom = EEPROMBase::get();
//     eeprom->get_calibration_parameter((unsigned char *)(&robot_calibration_parameter));
//     Robot_config_parameter config_parameter;
//     eeprom->get_config_parameter((unsigned char *)(&(config_parameter)));
//     pool_parameter.pool_length = config_parameter.pool_parameter.pool_length;
//     pool_parameter.pool_width = config_parameter.pool_parameter.pool_width;
//     pool_parameter.pool_area = config_parameter.pool_parameter.pool_area;
//     pool_parameter.pool_surface_material = config_parameter.pool_parameter.pool_surface_material;
//     pool_parameter.pool_depth = config_parameter.pool_parameter.pool_depth;
//     pool_parameter.pool_shape = config_parameter.pool_parameter.pool_shape;
//     pool_parameter.pool_floor_type = config_parameter.pool_parameter.pool_floor_type;
//     pool_parameter.pool_floor_max_angle = config_parameter.pool_parameter.pool_floor_max_angle;
//     state_parameter.robot_mode.cleaning_mode = config_parameter.cleaning_mode;
//     state_parameter.robot_mode.pool_shape_adaptive_route_enable = config_parameter.pool_shape_adaptive_route_enable;
//     state_parameter.robot_mode.default_floor_route_mode = config_parameter.default_floor_route_mode;
//     state_parameter.robot_mode.default_wall_route_mode = config_parameter.default_wall_route_mode;
//     state_parameter.robot_mode.default_combination_route_mode = config_parameter.default_combination_route_mode;
//     state_parameter.robot_mode.default_all_pool_route_mode = config_parameter.default_all_pool_route_mode;
//     // state_parameter->robot_mode.route_mode = DEFAULT_ROUTE_MODE;
//     // state_parameter->robot_mode.cleaning_mode = ALL_POOL_CLEANING_MODE;
//     // eeprom->Write_Data(ADDRESS_ROUTE_MODE, (uint8_t)state_parameter->robot_mode.route_mode);
//     // eeprom->load_version_data((unsigned char *)&product_info);
//     if(pool_parameter.pool_surface_material == POOL_SURFACE_SMOOTH)
//     {
//        basic_parameter.motor_configuration.pump_motor_pwm_baseline_waterline = PUMP_MOTOR_PWM_BASELINE_WATERLINE + 100;
//     }
//     else if(pool_parameter.pool_surface_material == POOL_SURFACE_ROUGH)
//     {
//         basic_parameter.motor_configuration.pump_motor_pwm_baseline_waterline = PUMP_MOTOR_PWM_BASELINE_WATERLINE - 50;
//         basic_parameter.motor_configuration.pump_motor_pwm_baseline_floor = PUMP_MOTOR_PWM_BASELINE_FLOOR - 50;
//         basic_parameter.motor_configuration.pump_motor_pwm_baseline_up_wall = PUMP_MOTOR_PWM_BASELINE_UP_WALL - 50;
//         basic_parameter.motor_configuration.pump_motor_pwm_baseline_down_wall = PUMP_MOTOR_PWM_BASELINE_DOWN_WALL - 50;
//     }

// #if SUPPORT_SCHEDULE
//     eeprom->load_schedule_data((uint8_t *)&(schedule_state.schedule_config));
//     // schedule_state.schedule_config.schedule_enable = true;
//     // schedule_state.schedule_config.interval_hours = 0;      // 清洗间隔时长
//     // schedule_state.schedule_config.cycle_time_minutes = 0;  // 清洗设置时长
// #endif
//     eeprom->load_total_running_time(state_parameter.total_running_time);
//     eeprom->load_total_washing_time(state_parameter.total_washing_time);
//     eeprom->load_fault_alarm_record((uint8_t *)robot_fault_alarm);
//     eeprom->load_serial_number(serial_number_data.serial_number, serial_number_data.length);
// #else // FLASH
//     Board::get()->get_config((unsigned char *)&static_parameter, sizeof(static_parameter));
// #endif
//     Board::get()->GetUID(product_info.uid);
// }

// void Data_holder::save_parameter()
// {
//     // Board::get()->set_config((unsigned char *)&static_parameter, sizeof(static_parameter));
// }

// void Data_holder::update_touch_wall_debug_state(unsigned int current_time)
// {
//     static unsigned int debug_state_inclination_touch_wall_detected_start_time = 0;
//     if(state_parameter.inclination_touch_wall_detected)
//     {
//         debug_state.inclination_touch_wall_detected = true;
//         state_bitmap.inclination_touch_wall_detected = 1;
//         debug_state_inclination_touch_wall_detected_start_time = current_time;
//     }
//     else if(debug_state_inclination_touch_wall_detected_start_time != 0 && 
//         current_time - debug_state_inclination_touch_wall_detected_start_time > 2000)
//     {
//         debug_state.inclination_touch_wall_detected = false;
//         state_bitmap.inclination_touch_wall_detected = 0;
//     }

//     static unsigned int debug_state_pwm_touch_wall_detected_start_time = 0;
//     if(state_parameter.pwm_touch_wall_detected)
//     {
//         debug_state.pwm_touch_wall_detected = true;
//         state_bitmap.pwm_touch_wall_detected = 1;
//         debug_state_pwm_touch_wall_detected_start_time = current_time;
//     }
//     else if(debug_state_pwm_touch_wall_detected_start_time != 0 && 
//         current_time - debug_state_pwm_touch_wall_detected_start_time > 2000)
//     {
//         debug_state.pwm_touch_wall_detected = false;
//         state_bitmap.pwm_touch_wall_detected = 0;
//     }
    
//     static unsigned int debug_state_variance_touch_wall_detected_start_time = 0;
//     if(state_parameter.variance_touch_wall_detected)
//     {
//         debug_state.variance_touch_wall_detected = true;
//         state_bitmap.variance_touch_wall_detected = 1;
//         debug_state_variance_touch_wall_detected_start_time = current_time;
//     }
//     else if(debug_state_variance_touch_wall_detected_start_time != 0 && 
//         current_time - debug_state_variance_touch_wall_detected_start_time > 2000)
//     {
//         debug_state.variance_touch_wall_detected = false;
//         state_bitmap.variance_touch_wall_detected = 0;
//     }

//     static unsigned int debug_state_no_head_up_after_touch_wall_detected_start_time = 0;
//     if(state_parameter.no_head_up_after_touch_wall_detected)
//     {
//         debug_state.no_head_up_after_touch_wall_detected = true;
//         state_bitmap.no_head_up_after_touch_wall_detected = 1;
//         debug_state_no_head_up_after_touch_wall_detected_start_time = current_time;
//     }
//     else if(debug_state_no_head_up_after_touch_wall_detected_start_time != 0 && 
//         current_time - debug_state_no_head_up_after_touch_wall_detected_start_time > 2000)
//     {
//         debug_state.no_head_up_after_touch_wall_detected = false;
//         state_bitmap.no_head_up_after_touch_wall_detected = 0;
//     }

//     static unsigned int debug_state_infrared_touch_wall_detected_start_time = 0;
//     if(state_parameter.infrared_touch_wall_detected)
//     {
//         state_bitmap.infrared_touch_wall_detected = 1;
//         debug_state_infrared_touch_wall_detected_start_time = current_time;
//     }
//     else if(debug_state_infrared_touch_wall_detected_start_time != 0 && 
//         current_time - debug_state_infrared_touch_wall_detected_start_time > 2000)
//     {
//         state_bitmap.infrared_touch_wall_detected = 0;
//     }
    
// }

// void Data_holder::update_debug_state(unsigned int current_time)
// {
//     state_bitmap.touch_wall_detection_enable = debug_state.touch_wall_detection_enable = state_parameter.touch_wall_detection_enable;
//     state_bitmap.pwm_touch_wall_detection_enable = debug_state.pwm_touch_wall_detection_enable = state_parameter.pwm_touch_wall_detection_enable;
//     state_bitmap.variance_touch_wall_detection_enable = state_parameter.variance_touch_wall_detection_enable;
    
//     state_bitmap.touch_wall_detected = debug_state.touch_wall_detected = state_parameter.touch_wall_detected;
//     state_bitmap.touch_wall_timeout = debug_state.touch_wall_timeout = state_parameter.touch_wall_timeout;
//     state_bitmap.touch_wall_detection_delay_enable = debug_state.touch_wall_detection_delay_enable = state_parameter.touch_wall_detection_delay_enable;
//     state_bitmap.infrared_waterline_detected = state_parameter.infrared_waterline_detected;
//     state_bitmap.infrared_corner_detected = state_parameter.infrared_corner_detected;
//     debug_state.touch_wall_count = state_parameter.touch_wall_count;

//     state_bitmap.down_wall_stuck = debug_state.down_wall_stuck = state_parameter.down_wall_stuck;
//     state_bitmap.up_wall_stuck = debug_state.up_wall_stuck = state_parameter.up_wall_stuck;
//     state_bitmap.stuck_on_floor = debug_state.stuck_on_floor = state_parameter.stuck_on_floor;
//     state_bitmap.stuck_on_floor_detected_by_touch_wall_detector = debug_state.stuck_on_floor_detected_by_touch_wall_detector = state_parameter.stuck_on_floor_detected_by_touch_wall_detector;
//     state_bitmap.stuck_on_floor_detected_by_turn_direction_timeout_detector = debug_state.stuck_on_floor_detected_by_turn_direction_timeout_detector = state_parameter.stuck_on_floor_detected_by_turn_direction_timeout_detector;
//     state_bitmap.tilt_stuck = state_parameter.tilt_stuck;
//     state_bitmap.unable_tilt_wash_on_wall = debug_state.unable_tilt_wash_on_wall = state_parameter.unable_tilt_wash_on_wall;
//     state_bitmap.near_waterline = state_parameter.near_waterline;

//     static unsigned int debug_state_infrared_waterline_detected_start_time = 0;
//     if(state_parameter.infrared_waterline_detected)
//     {
//         state_bitmap.infrared_waterline_detected = 1;
//         debug_state_infrared_waterline_detected_start_time = current_time;
//     }
//     else if(debug_state_infrared_waterline_detected_start_time != 0 && 
//         current_time - debug_state_infrared_waterline_detected_start_time > 2000)
//     {
//         state_bitmap.infrared_waterline_detected = 0;
//     }

//     static unsigned int debug_state_infrared_corner_detected_start_time = 0;
//     if(state_parameter.infrared_corner_detected)
//     {
//         state_bitmap.infrared_corner_detected = 1;
//         debug_state_infrared_corner_detected_start_time = current_time;
//     }
//     else if(debug_state_infrared_corner_detected_start_time != 0 && 
//         current_time - debug_state_infrared_corner_detected_start_time > 2000)
//     {
//         state_bitmap.infrared_corner_detected = 0;
//     }

//     static unsigned int debug_state_reach_top_start_time = 0;
//     if(state_parameter.reach_top)
//     {
//         debug_state.reach_top = true;
//         state_bitmap.reach_top = 1;
//         debug_state_reach_top_start_time = current_time;
//     }
//     else if(debug_state_reach_top_start_time != 0 && 
//         current_time - debug_state_reach_top_start_time > 2000)
//     {
//         debug_state.reach_top = false;
//         state_bitmap.reach_top = 0;
//     }

//     static unsigned int debug_state_reach_bottom_start_time = 0;
//     if(state_parameter.reach_bottom)
//     {
//         debug_state.reach_bottom = true;
//         state_bitmap.reach_bottom = 1;
//         debug_state_reach_bottom_start_time = current_time;
//     }
//     else if(debug_state_reach_bottom_start_time != 0 && 
//         current_time - debug_state_reach_bottom_start_time > 2000)
//     {
//         debug_state.reach_bottom = false;
//         state_bitmap.reach_bottom = 0;
//     }

//     static unsigned int debug_state_reach_waterline_start_time = 0;
//     if(state_parameter.reach_waterline)
//     {
//         debug_state.reach_waterline = true;
//         state_bitmap.reach_waterline = 1;
//         debug_state_reach_waterline_start_time = current_time;
//     }
//     else if(debug_state_reach_waterline_start_time != 0 && 
//         current_time - debug_state_reach_waterline_start_time > 2000)
//     {
//         debug_state.reach_waterline = false;
//         state_bitmap.reach_waterline = 0;
//     }

//     if(state_parameter.up_wall_start_time != 0)
//     {
//         debug_state.start_up_wall = true;
//         debug_state.start_down_wall = false;
//         // debug_state.stop_up_wall = false;
//         state_bitmap.start_up_wall = 1;
//         state_bitmap.start_down_wall = 0;
//     }
//     else
//     {
//         debug_state.start_up_wall = false;
//         state_bitmap.start_up_wall = 0;
//     }

//     if(state_parameter.down_wall_start_time != 0)
//     {
//         debug_state.start_down_wall = true;
//         debug_state.start_up_wall = false;
//         // debug_state.stop_down_wall = false;
//         state_bitmap.start_down_wall = 1;
//         state_bitmap.start_up_wall = 0;
//     }
//     else
//     {
//         debug_state.start_down_wall = false;
//         state_bitmap.start_down_wall = 0;
//     }
//     state_bitmap.depth_stable = debug_state.depth_stable = state_parameter.depth_stable;
//     state_bitmap.climb_wall = debug_state.climb_wall = state_parameter.climb_wall;
//     state_bitmap.direction_reversed = debug_state.direction_reversed = state_parameter.direction_reversed;
//     state_bitmap.need_detect_corner = debug_state.need_detect_corner = state_parameter.need_detect_corner;
//     state_bitmap.low_power_state = state_parameter.low_power_state;


//     debug_state.total_start_floor_count = state_parameter.total_start_floor_count;
//     debug_state.mode_start_floor_index = state_parameter.mode_start_floor_index;
//     debug_state.total_start_wall_count = state_parameter.total_start_wall_count;
//     debug_state.mode_start_wall_index = state_parameter.mode_start_wall_index;

//     debug_state.down_wall_period = state_parameter.down_wall_period;
//     debug_state.up_wall_period = state_parameter.up_wall_period;
//     debug_state.on_floor_period = state_parameter.on_floor_period[0];
//     debug_state.pool_center_period = state_parameter.pool_center_period;
    
//     debug_state.total_running_time = state_parameter.total_running_time;
// }

// void Data_holder::update_debug_action()
// {
//     debug_action_parameter.action = action_parameter.action;
//     debug_action_parameter.movement_direction = action_parameter.movement_direction;
//     debug_action_parameter.yaw_baseline = action_parameter.yaw_baseline;
//     debug_action_parameter.yaw_target = action_parameter.yaw_target;
//     debug_action_parameter.beta_baseline = action_parameter.beta_baseline;
//     debug_action_parameter.beta_target = action_parameter.beta_target;
//     debug_action_parameter.depth_target = action_parameter.depth_target;
//     debug_action_parameter.out_of_water_action_enable = action_parameter.out_of_water_action_enable;
//     debug_action_parameter.pump_spout_action_enable = action_parameter.pump_spout_action_enable;
//     debug_action_parameter.suck_air_action_enable = action_parameter.suck_air_action_enable;
//     debug_action_parameter.turn_direction_stuck_count = action_parameter.turn_direction_stuck_count;
//     debug_action_parameter.continuous_stuck_count = action_parameter.continuous_stuck_count;
//     debug_action_parameter.unable_up_wall_action_timeout_count = action_parameter.unable_up_wall_action_timeout_count;
//     debug_action_parameter.action_start_time = action_parameter.action_start_time;
// }
